/*
 * @Author: LVGRAPE
 * @Date: 2023-12-13 11:26:01
 * @LastEditTime: 2025-07-02 18:09:05
 * @LastEditors: LVGRAPE
 * @Description:
 * @FilePath: \zino-fc-v4\ZINO\hardware\Icm426xx\platform\lcm42688P_read_polling.c
 * 要啥没啥，爱咋咋的
 */

#include "Icm426xxTransport.h"
#include "Icm426xxDriver_HL.h"
#include "rtthread.h"
#include "drv_spi.h"
#include "zino.h"
#include "fc_sensors.h"

#define DBG_TAG "ICM6288P"
#define DBG_LVL DBG_LOG
#include <rtdbg.h>

#define SERIF_TYPE ICM426XX_UI_SPI4
// NOTE - 测试板用的qmi的总线,正常板子用的lsm的总线
#define ICM42688P_BUS_NAME "spi_icm"
#define ICM_I2C_ADDR 0x68
#define USE_CLK_IN 0
#define SENSOR_ODR 0

spi_dev_t icm_spi = {
    .cs_pin = ICM_CSN_PIN,
    .mode = RT_SPI_MODE_3 | RT_SPI_MSB,
    .data_width = 8,
    .max_hz = 24 * 1000 * 1000,
};

const float gyro_full_scale_factor[8] = {
    16.4f,   //+/-2000dps
    32.8f,   //+/-1000dps
    65.5f,   //+/-500dps
    131.f,   //+/-250dps
    256.f,   //+/-125dps
    524.3f,  //+/-62.5dps
    1048.6f, //+/-31.25dps
    2097.2f, //+/-15.625dsp
};

const uint16_t accel_full_scale_factor[4] = {
    2048, //+/-16g
    4096, //+/-8g
    8192, //+/-4g
    16384 //+/-2g
};

static struct inv_icm426xx_serif icm42688p_dev;
/* Just a handy variable to handle the icm426xx object */
static struct inv_icm426xx icm_driver;
/* Just a handy variable to handle the accel full scale factor */
static uint16_t accel_fs_factor = 2048;
/* Just a handy variable to handle the gyro full scale factor */
static float gyro_fs_factor = 16.4f;
float get_acc_scale_factor(void)
{
    return 1.f / (float)accel_fs_factor;
}
float get_gyro_scale_factor(void)
{
    return 1.f / gyro_fs_factor;
}
/**
 * @brief 读寄存器
 *
 * @param serif
 * @param reg
 * @param rbuffer
 * @param rlen
 * @return int
 */
int inv_io_hal_read_reg(struct inv_icm426xx_serif *serif, uint8_t reg, uint8_t *rbuffer,
                        uint32_t rlen)
{
    switch (serif->serif_type)
    {
    case ICM426XX_AUX1_SPI3:
    case ICM426XX_AUX1_SPI4:
    case ICM426XX_AUX2_SPI3:
    case ICM426XX_UI_SPI4:
        reg |= 0x80;
        return spi_send_then_receive(&icm_spi, &reg, 1, rbuffer, rlen);
    case ICM426XX_UI_I2C:
        break;
    default:
        return -1;
    }
    return 0;
}
/**
 * @brief 写寄存器
 *
 * @param serif
 * @param reg
 * @param wbuffer
 * @param wlen
 * @return int
 */
int inv_io_hal_write_reg(struct inv_icm426xx_serif *serif, uint8_t reg, const uint8_t *wbuffer,
                         uint32_t wlen)
{
    int rc;

    switch (serif->serif_type)
    {
    case ICM426XX_AUX1_SPI3:
    case ICM426XX_AUX1_SPI4:
    case ICM426XX_AUX2_SPI3:
    case ICM426XX_UI_SPI4:
        for (uint32_t i = 0; i < wlen; i++)
        {
            uint8_t regAddr = reg + i;
            rc = spi_send_then_send(&icm_spi, &regAddr, 1, (uint8_t *)&wbuffer[i], 1);
            if (rc)
                return rc;
        }
        return 0;
    case ICM426XX_UI_I2C:
        break;
    default:
        return -1;
    }
    return 0;
}
/**
 * @brief 初始化spi,i2c接口给icm42688用，i2c没有调试
 *
 * @param icm_serif
 * @return int
 */
static int platform_init(struct inv_icm426xx_serif *icm_serif)
{

    /* Initialize serial inteface between MCU and Icm426xx */
    icm_serif->context = 0; /* no need */
    icm_serif->read_reg = inv_io_hal_read_reg;
    icm_serif->write_reg = inv_io_hal_write_reg;
    icm_serif->max_read = 1024 * 32;  /* maximum number of bytes allowed per serial read */
    icm_serif->max_write = 1024 * 32; /* maximum number of bytes allowed per serial write */
    icm_serif->serif_type = SERIF_TYPE;

    return RT_EOK;
}
void inv_icm426xx_sleep_us(uint32_t us)
{
    if (us < 1000)
        rt_hw_us_delay(us);
    else
        rt_thread_mdelay(us / 1000);
}
/**
 * @brief 初始化：上电，验证id
 *
 * @param icm_serif
 * @return int
 */
int SetupInvDevice(struct inv_icm426xx_serif *icm_serif)
{
    int rc = 0;
    uint8_t who_am_i;

    /* Initialize device */
    LOG_I("Initialize Icm42688p");
    rc = inv_icm426xx_init(&icm_driver, icm_serif, NULL);
    if (rc != RT_EOK)
    {
        LOG_E("!!! ERROR : failed to initialize Icm426xx.");
        return rc;
    }

    /* Check WHOAMI */
    LOG_I("Check Icm426xx whoami value");

    rc = inv_icm426xx_get_who_am_i(&icm_driver, &who_am_i);
    if (rc != INV_ERROR_SUCCESS)
    {
        LOG_E("!!! ERROR : failed to read Icm426xx whoami value.");
        return rc;
    }

    if (who_am_i != ICM_WHOAMI)
    {
        LOG_E("!!! ERROR :  bad WHOAMI value. Got 0x%02x (expected: 0x%02x)",
              who_am_i, ICM_WHOAMI);
        return INV_ERROR;
    }

    return rc;
}
/**
 * @brief 配置量程，ODR, LFP
 *
 * @return int
 */
int ConfigureInvDevice()
{
    int rc = 0;
    uint8_t data;
    /* Set Pulse to support high rates */
    rc |= inv_icm426xx_read_reg(&icm_driver, MPUREG_INT_CONFIG1, 1, &data);
    data |=
        ((uint8_t)ICM426XX_INT_TPULSE_DURATION_8_US) | ((uint8_t)ICM426XX_INT_TDEASSERT_DISABLED);
    rc |= inv_icm426xx_write_reg(&icm_driver, MPUREG_INT_CONFIG1, 1, &data);

    /* Disable fifo usage, data will be read from sensors registers*/
    rc |= inv_icm426xx_configure_fifo(&icm_driver, INV_ICM426XX_FIFO_DISABLED);
    if (rc != INV_ERROR_SUCCESS)
    {
        LOG_E("!!! ERROR : failed to initialize Icm426xx.");
        return rc;
    }

#if defined(ICM42633) && USE_CLK_IN
    /*
     * ICM42633 is a triple interface device. To access CLKIN, AUX2 interface needs to be disabled.
     * Use INV_ICM426XX_DUAL_INTERFACE mode. The following mode are also compatible:
     *  - INV_ICM426XX_SINGLE_INTERFACE
     *  - INV_ICM426XX_DUAL_INTERFACE_SPI4
     */
    rc |= inv_icm426xx_interface_change_procedure(&icm_driver, INV_ICM426XX_DUAL_INTERFACE);
#endif
    rc |= inv_icm426xx_enable_clkin_rtc(&icm_driver, USE_CLK_IN);

    rc |= inv_icm426xx_set_accel_fsr(&icm_driver, ICM426XX_ACCEL_CONFIG0_FS_SEL_4g);
    rc |= inv_icm426xx_set_gyro_fsr(&icm_driver, ICM426XX_GYRO_CONFIG0_FS_SEL_1000dps);

    rc |= inv_icm426xx_set_accel_frequency(&icm_driver, ICM426XX_ACCEL_CONFIG0_ODR_1_KHZ);
    rc |= inv_icm426xx_set_gyro_frequency(&icm_driver, ICM426XX_GYRO_CONFIG0_ODR_1_KHZ);
    // rc |= inv_icm426xx_set_accel_frequency(&icm_driver, ICM426XX_ACCEL_CONFIG0_ODR_1_KHZ);
    // rc |= inv_icm426xx_set_gyro_frequency(&icm_driver, ICM426XX_GYRO_CONFIG0_ODR_1_KHZ);

    // #if SENSOR_ODR == HIGH_RATE_8KHZ
    //     rc |= inv_icm426xx_set_accel_frequency(&icm_driver, ICM426XX_ACCEL_CONFIG0_ODR_8_KHZ);
    //     rc |= inv_icm426xx_set_gyro_frequency(&icm_driver, ICM426XX_GYRO_CONFIG0_ODR_8_KHZ);
    // #elif SENSOR_ODR == HIGH_RATE_16KHZ
    //     rc |= inv_icm426xx_set_accel_frequency(&icm_driver, ICM426XX_ACCEL_CONFIG0_ODR_16_KHZ);
    //     rc |= inv_icm426xx_set_gyro_frequency(&icm_driver, ICM426XX_GYRO_CONFIG0_ODR_16_KHZ);
    // #elif SENSOR_ODR == HIGH_RATE_32KHZ
    //     rc |= inv_icm426xx_set_accel_frequency(&icm_driver, ICM426XX_ACCEL_CONFIG0_ODR_32_KHZ);
    //     rc |= inv_icm426xx_set_gyro_frequency(&icm_driver, ICM426XX_GYRO_CONFIG0_ODR_32_KHZ);
    // #endif
    // NOTE 开启低噪声模式
    rc |= inv_icm426xx_enable_gyro_low_noise_mode(&icm_driver);
    rc |= inv_icm426xx_enable_accel_low_noise_mode(&icm_driver);

    // rc |= inv_icm426xx_set_accel_ln_bw(&icm_driver, ICM426XX_GYRO_ACCEL_CONFIG0_GYRO_FILT_BW_20);
    //  rc |= inv_icm426xx_set_gyro_ln_bw(&icm_driver, ICM426XX_GYRO_ACCEL_CONFIG0_GYRO_FILT_BW_40);

    // NOTE 读数量程系数，原始数据除以这个数能得真实值
    ICM426XX_ACCEL_CONFIG0_FS_SEL_t accel_fsr;
    ICM426XX_GYRO_CONFIG0_FS_SEL_t gyro_fsr;
    rc |= inv_icm426xx_get_accel_fsr(&icm_driver, &accel_fsr);
    rc |= inv_icm426xx_get_gyro_fsr(&icm_driver, &gyro_fsr);
    accel_fs_factor = accel_full_scale_factor[(accel_fsr >> BIT_ACCEL_CONFIG0_FS_SEL_POS) % 4];
    gyro_fs_factor = gyro_full_scale_factor[(gyro_fsr >> BIT_GYRO_CONFIG0_FS_SEL_POS) % 8];
    LOG_D("accel_fs_factor:%d", accel_fs_factor);
    LOG_D("gyro_fs_factor:%d.%01d", (int)gyro_fs_factor, (int)(gyro_fs_factor * 10) % 10);

    /* Wait the maximum startup time in case gyro is enbaled */
    inv_icm426xx_sleep_us(ICM426XX_GYR_STARTUP_TIME_US);

    return rc;
}
uint64_t inv_icm426xx_get_time_us(void)
{
    return rt_tick_get() * 1000;
}
/**
 * @brief 配置icm42688,上电，量程，odr
 *
 * @return int
 */
int icm42688p_read_polling_init(void)
{
    rt_thread_mdelay(25); // NOTE waite for power stable
    if (platform_init(&icm42688p_dev) != RT_EOK)
    {
        LOG_E("icm42688p_read_polling_init failed!");
        return -RT_ERROR;
    }
    LOG_I("platform_init ok!");
    if (SetupInvDevice(&icm42688p_dev) != RT_EOK)
    {
        LOG_E("SetupInvDevice error!");
        return -RT_ERROR;
    }
    else
    {
        LOG_I("SetupInvDevice ok!");
    }

    if (ConfigureInvDevice() != RT_EOK)
    {
        LOG_E("ConfigureInvDevice error!");
        return -RT_ERROR;
    }
    else
    {
        LOG_I("ConfigureInvDevice ok!");
    }
    // int16_t readbuffer[6];
    // for (uint16_t i = 0; i < 10; i++)
    // {
    //     inv_io_hal_read_reg(&icm42688p_dev, MPUREG_ACCEL_DATA_X0_UI, (uint8_t *)readbuffer, 12);
    //     rt_kprintf("acc: %4d, %4d, %4d, gyro: %4d, %4d, %4d\n",
    //                readbuffer[0], readbuffer[1], readbuffer[2], readbuffer[3], readbuffer[4], readbuffer[5]);

    //     rt_thread_mdelay(5);
    // }
    return RT_EOK;
}
/**
 * @brief
 *
 * @param ax mg
 * @param ay mg
 * @param az mg
 * @return int
 */
int icm42688p_accel_read_polling(int32_t *ax, int32_t *ay, int32_t *az)
{
    uint8_t accelBuffer[6];
    int ret = 0;
    ret = inv_io_hal_read_reg(&icm42688p_dev, MPUREG_ACCEL_DATA_X0_UI, (uint8_t *)accelBuffer, 6);
    int16_t raw_ax = (int16_t)(accelBuffer[0] << 8 | accelBuffer[1]);
    int16_t raw_ay = (int16_t)(accelBuffer[2] << 8 | accelBuffer[3]);
    int16_t raw_az = (int16_t)(accelBuffer[4] << 8 | accelBuffer[5]);
    /**
     * @brief NOTE AXIS:
     *         X^
     *          |
     *  Y <-----|
     */
    *ay = -(long)raw_ax * 1000 / accel_fs_factor;
    *ax = -(long)raw_ay * 1000 / accel_fs_factor;
    *az = -(long)raw_az * 1000 / accel_fs_factor;
    // rt_kprintf("raw acc: %d \t %d \t %d\n, ",raw_ax,raw_ay,raw_az);
    return ret;
}
int icm42688p_accel_read_polling2(int16_t *acc)
{
    uint8_t accelBuffer[6];
    int ret = 0;
    ret = inv_io_hal_read_reg(&icm42688p_dev, MPUREG_ACCEL_DATA_X0_UI, (uint8_t *)accelBuffer, 6);
    acc[1] = -(int16_t)(accelBuffer[0] << 8 | accelBuffer[1]);
    acc[0] = -(int16_t)(accelBuffer[2] << 8 | accelBuffer[3]);
    acc[2] = -(int16_t)(accelBuffer[4] << 8 | accelBuffer[5]);
    /**
     * @brief NOTE AXIS:
     *         X^
     *          |
     *  Y <-----|
     */
    // acc[1] = -(float)raw_ax / (float)accel_fs_factor;
    // acc[0] = -(float)raw_ay / (float)accel_fs_factor;
    // acc[2] = -(float)raw_az / (float)accel_fs_factor;
    // rt_kprintf("raw acc: %d \t %d \t %d\n, ",raw_ax,raw_ay,raw_az);
    return ret;
}
int icm42688p_gyro_read_polling2(int16_t *gyro)
{
    uint8_t gyroBuffer[6];
    int ret = 0;
    ret = inv_io_hal_read_reg(&icm42688p_dev, MPUREG_GYRO_DATA_X0_UI, (uint8_t *)gyroBuffer, 6);
    gyro[1] = -(int16_t)(gyroBuffer[0] << 8 | gyroBuffer[1]);
    gyro[0] = -(int16_t)(gyroBuffer[2] << 8 | gyroBuffer[3]);
    gyro[2] = -(int16_t)(gyroBuffer[4] << 8 | gyroBuffer[5]);
    /**
     * @brief NOTE AXIS:
     *         X^
     *          |
     *  Y <-----|
     */
    // gyro[1]= -(float)raw_gx / gyro_fs_factor;
    // gyro[0]= -(float)raw_gy / gyro_fs_factor;
    // gyro[2]= -(float)raw_gz / gyro_fs_factor;
    // rt_kprintf("raw gyro: %d \t %d \t %d\n, ",raw_gx,raw_gy,raw_gz);
    return ret;
}

int sensor_imc42688p_poll_data(fc_sensor_data_t *sensor)
{
    uint8_t buffer[12];
    int ret = 0;
    ret = inv_io_hal_read_reg(&icm42688p_dev, MPUREG_ACCEL_DATA_X0_UI, (uint8_t *)buffer, 12);
    sensor->acc.accRaw.axis[1] = -(int16_t)(buffer[0] << 8 | buffer[1]);
    sensor->acc.accRaw.axis[0] = -(int16_t)(buffer[2] << 8 | buffer[3]);
    sensor->acc.accRaw.axis[2] = -(int16_t)(buffer[4] << 8 | buffer[5]);
    sensor->gyro.gyroRaw.axis[1] = -(int16_t)(buffer[6] << 8 | buffer[7]);
    sensor->gyro.gyroRaw.axis[0] = -(int16_t)(buffer[8] << 8 | buffer[9]);
    sensor->gyro.gyroRaw.axis[2] = -(int16_t)(buffer[10] << 8 | buffer[11]);
    return ret;
}
/**
 * @brief
 *
 * @param gx mDps*1000
 * @param gy mDps*1000
 * @param gz mDps*1000
 * @return int
 */
int icm42688p_gyro_read_polling(int32_t *gx, int32_t *gy, int32_t *gz)
{
    uint8_t gyroBuffer[6];
    int ret = 0;
    ret = inv_io_hal_read_reg(&icm42688p_dev, MPUREG_GYRO_DATA_X0_UI, (uint8_t *)gyroBuffer, 6);
    int16_t raw_gx = (int16_t)(gyroBuffer[0] << 8 | gyroBuffer[1]);
    int16_t raw_gy = (int16_t)(gyroBuffer[2] << 8 | gyroBuffer[3]);
    int16_t raw_gz = (int16_t)(gyroBuffer[4] << 8 | gyroBuffer[5]);
    /**
     * @brief NOTE AXIS:
     *         X^
     *          |
     *  Y <-----|
     */
    *gy = -(float)raw_gx * 1000000.f / gyro_fs_factor;
    *gx = -(float)raw_gy * 1000000.f / gyro_fs_factor;
    *gz = -(float)raw_gz * 1000000.f / gyro_fs_factor;
    // rt_kprintf("raw gyro: %d \t %d \t %d\n, ",raw_gx,raw_gy,raw_gz);
    return ret;
}

int icm42688p_gyroraw_read_polling(int16_t *gx, int16_t *gy, int16_t *gz)
{
    int ret = 0;
    uint8_t gyroBuffer[6];
    ret = inv_io_hal_read_reg(&icm42688p_dev, MPUREG_GYRO_DATA_X0_UI, (uint8_t *)gyroBuffer, 6);
    *gx = (int16_t)(gyroBuffer[0] << 8 | gyroBuffer[1]);
    *gy = (int16_t)(gyroBuffer[2] << 8 | gyroBuffer[3]);
    *gz = (int16_t)(gyroBuffer[4] << 8 | gyroBuffer[5]);
    return ret;
}

int icm42688p_accraw_read_polling(int16_t *ax, int16_t *ay, int16_t *az)
{
    int ret = 0;
    uint8_t accBuffer[6];
    ret = inv_io_hal_read_reg(&icm42688p_dev, MPUREG_ACCEL_DATA_X0_UI, (uint8_t *)accBuffer, 6);
    *ax = (int16_t)(accBuffer[0] << 8 | accBuffer[1]);
    *ay = (int16_t)(accBuffer[2] << 8 | accBuffer[3]);
    *az = (int16_t)(accBuffer[4] << 8 | accBuffer[5]);
    return ret;
}

// ZINO_APP_EXPORT(icm42688p_read_polling_init);
